#include "firfilter.h"
#include <cmath>
#include <cstring>


/**
 * Init the filter operation
 */
firfilter::firfilter()
{

}

/**
 * Destructor
 */
firfilter::~firfilter()
{

}

/*
 * Init the filter operation
 */
void firfilter::init(tFirMainStructure* tFirMainStructure_)
{
    for (int i=0; i < FIR_ORDER; i++)
    {
        tFirMainStructure_->history[i] = 0;
    }

    for (int j=0; j < FIR_ORDER; j++)
    {
        tFirMainStructure_->coefs[j] = fir_init_coefs[j];
    }

    tFirMainStructure_->hist_pos = 0;

    tFirMainStructure_->coef_num = FIR_ORDER;
}

/**
 * IIRFILTER
 * 
 *  y(n) = b_0*x(n) + b_1*x(n-1) + ... + b_29*x(n-29)
 */   
void firfilter::filter(
    int blockSize,  						/* Number of output samples */
    float* in,								/* Pointer to input array (n) */
    float* out,								/* Pointer to output data (y) */
    tFirMainStructure* tFirMainStructure_)	/* Pointer to Fir structure */
{
    int i;
    int j;
    int hist_pos;
    float sum = 0;

    for (i = 0; i < blockSize; i++)
    {
        hist_pos = tFirMainStructure_->hist_pos;

        tFirMainStructure_->history[hist_pos] = in[i];
        tFirMainStructure_->hist_pos++;

        if(tFirMainStructure_->hist_pos == FIR_ORDER)
        {
            tFirMainStructure_->hist_pos = 0;
        }

        sum = 0.00;
        for(j=0; j<FIR_ORDER; j++)
        {
            sum += (tFirMainStructure_->coefs[j]) * (tFirMainStructure_->history[hist_pos]);
            hist_pos--;

            if(hist_pos < 0)
            {
                hist_pos = FIR_ORDER-1;
            }
        }
        out[i] = sum;
    }
}

/*
 * Reset
 */
void firfilter::reset(tFirMainStructure* tFirMainStructure_)
{
    for (int i=0; i < FIR_ORDER; i++)
    {
        tFirMainStructure_->history[i] = 0;
    }

    for (int j=0; j < FIR_ORDER; j++)
    {
        tFirMainStructure_->coefs[j] = fir_init_coefs[j];
    }

    tFirMainStructure_->hist_pos = 0;

    tFirMainStructure_->coef_num = FIR_ORDER;
}
